#include<iostream>
#include<iomanip>
#include<cstdlib>
#include<cmath>

#include<formic/vector/vector.h>
#include<formic/vector/matrix.h>

int main() {

  typedef formic::Vector<double,int> FDVec;
  typedef formic::Matrix<double,int> FDMat;

  {
    FDVec v0(4, 1.0);      std::cout << "v0 constructed" << std::endl << std::endl;
    FDVec v1 = v0.clone(); std::cout << "v1 constructed" << std::endl << std::endl;
    FDVec v2;              std::cout << "v2 constructed" << std::endl << std::endl;
    v2 = v0 + v1;          std::cout << "1st add complete" << std::endl << std::endl;
    v2 = v1 + v2;          std::cout << "2nd add complete" << std::endl << std::endl;
    v2 = -v1 / v2;         std::cout << "1st division complete" << std::endl << std::endl;
    for (int i = 0; i < v2.size(); i++)
      std::cout << boost::format("%.2f") % v2[i] << std::endl;
    std::cout << std::endl;
  }
  std::cout << "vectors have been destroyed" << std::endl << std::endl;

  {
    FDMat m0(4, 4, 0.0); std::cout << "m0 constructed" << std::endl << std::endl;
    for (int i = 0; i < m0.rows(); i++)
      m0(i,i) = 1.0;
    FDMat m1 = 2.0 - m0;
    std::cout << "m1 prepared" << std::endl << std::endl;
    m1 = m1 + m1; std::cout << "m1 added" << std::endl << std::endl;
    m1 = m1 + m1; std::cout << "m1 added" << std::endl << std::endl;
    m1 = m1 + m1; std::cout << "m1 added" << std::endl << std::endl;
    for (int i = 0; i < m1.rows(); i++) {
      for (int j = 0; j < m1.cols(); j++)
        std::cout << boost::format(" %.2f") % m1(i,j);
      std::cout << std::endl;
    }
    std::cout << std::endl;
  }
  std::cout << "matrices have been destroyed" << std::endl << std::endl;

  formic::VectorLock<double,int>::garbage_collect();

  // test matrix adjoint
  {
    const int n = 3;
    const double dt = 1.0e-8;
    formic::Matrix<double,size_t> m(n, n);
    m(0,0) =  1.2;   m(0,1) =  0.3;   m(0,2) =  0.4;
    m(1,0) = -0.2;   m(1,1) =  0.9;   m(1,2) =  0.1;
    m(2,0) =  0.5;   m(2,1) =  0.4;   m(2,2) = -2.3;
    formic::Matrix<double,size_t> a(n, n);
    a(0,0) =  0.0;   a(0,1) =  0.0;   a(0,2) =  0.0;
    a(1,0) =  0.0;   a(1,1) =  0.0;   a(1,2) =  1.0;
    a(2,0) =  0.0;   a(2,1) =  0.0;   a(2,2) =  0.0;
    formic::Matrix<double,size_t> ma, me0, me1;
    ma = formic::matrix_exponent_der_adj(m, a, 1.0e-15);
    me0 = formic::matrix_exponent(m, 1.0e-15);
    m(1,0) += dt;
    me1 = formic::matrix_exponent(m, 1.0e-15);
    m(1,0) -= dt;
    std::cout << "derivative of exponentiated matrix's (1,2) element w.r.t. original matrix's (1,0) element" << std::endl;
    std::cout << "finite diff:  " << std::setprecision(15) << ( ( me1(1,2) - me0(1,2) ) / dt ) << std::endl;
    std::cout << "der adjoint:  " << std::setprecision(15) << ma(1,0) << std::endl;
    if ( std::abs( ma(1,0) - ( ( me1(1,2) - me0(1,2) ) / dt ) ) > 1.0e-8 ) {
      std::cout << "FAILURE:  derivative adjoint and finite difference do not match" << std::endl;
      std::cout << std::endl;
      return EXIT_FAILURE;
    }
    std::cout << std::endl;
    m(0,0) += dt;
    me1 = formic::matrix_exponent(m, 1.0e-15);
    m(0,0) -= dt;
    std::cout << "derivative of exponentiated matrix's (1,2) element w.r.t. original matrix's (0,0) element" << std::endl;
    std::cout << "finite diff:  " << std::setprecision(15) << ( ( me1(1,2) - me0(1,2) ) / dt ) << std::endl;
    std::cout << "der adjoint:  " << std::setprecision(15) << ma(0,0) << std::endl;
    if ( std::abs( ma(0,0) - ( ( me1(1,2) - me0(1,2) ) / dt ) ) > 1.0e-8 ) {
      std::cout << "FAILURE:  derivative adjoint and finite difference do not match" << std::endl;
      std::cout << std::endl;
      return EXIT_FAILURE;
    }
    std::cout << std::endl;
  }

//  v2 = v0 + v1;
//  v2 = v0 + v1;
//  v2 = v0 + v1;
//  v2 = v0 + v1;
//  v2 = v0 + v1;
//  v2 = v0 + v1;
//  v2 = v0 + v1;
//  v2 = v0 + v1;

  //std::cout << boost::format("v0 size = %i, v1 size = %i") % v0.size() % v1.size() << std::endl;

  return EXIT_SUCCESS;

}
